#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped


class PosePublisher(Node):

    def __init__(self):
        super().__init__('pose_publisher_node')

        # 创建两个 Publisher，分别发布到 /paper_pose 和 /yogurt_pose 话题
        self.paper_pose_pub = self.create_publisher(PoseStamped, '/paper_pose', 10)
        self.yogurt_pose_pub = self.create_publisher(PoseStamped, '/yogurt_pose', 10)

        # 定时器设置，每0.1秒（10Hz）发布消息
        self.timer = self.create_timer(0.1, self.publish_pose_messages)

        # 模拟数据：/paper_pose 的位姿信息
        self.paper_pose_data = {
            'header': {
                'seq': 15850,
                'stamp': {
                    'secs': 1736498668,
                    'nsecs': 96731185
                },
                'frame_id': "base_link"
            },
            'pose': {
                'position': {
                    'x': 0.52,
                    'y': -0.18,
                    'z': 0.72
                },
                'orientation': {
                    'x': -1,
                    'y': -1,
                    'z': -1,
                    'w': -1
                }
            }
        }

        # 模拟数据：/yogurt_pose 的位姿信息
        self.yogurt_pose_data = {
            'header': {
                'seq': 15851,  # 注意：seq 需要唯一
                'stamp': {
                    'secs': 1736498669,  # 时间戳可以不同
                    'nsecs': 96731186
                },
                'frame_id': "base_link"
            },
            'pose': {
                'position': {
                    'x': 0.59,  # 不同的位置
                    'y': 0.038,
                    'z': 0.723
                },
                'orientation': {
                    'x': -1,  # 不同的姿态
                    'y': -1,
                    'z': -1,
                    'w': -1
                }
            }
        }

    def publish_pose_messages(self):
        # 发布 /paper_pose 消息
        self.publish_pose(self.paper_pose_data, self.paper_pose_pub)

        # 发布 /yogurt_pose 消息
        self.publish_pose(self.yogurt_pose_data, self.yogurt_pose_pub)

    def publish_pose(self, pose_data, publisher):
        # 创建 PoseStamped 消息
        pose_msg = PoseStamped()

        # 设置 header
        pose_msg.header.seq = pose_data['header']['seq']
        pose_msg.header.stamp.sec = pose_data['header']['stamp']['secs']
        pose_msg.header.stamp.nanosec = pose_data['header']['stamp']['nsecs']
        pose_msg.header.frame_id = pose_data['header']['frame_id']

        # 设置 pose
        pose_msg.pose.position.x = pose_data['pose']['position']['x']
        pose_msg.pose.position.y = pose_data['pose']['position']['y']
        pose_msg.pose.position.z = pose_data['pose']['position']['z']

        pose_msg.pose.orientation.x = pose_data['pose']['orientation']['x']
        pose_msg.pose.orientation.y = pose_data['pose']['orientation']['y']
        pose_msg.pose.orientation.z = pose_data['pose']['orientation']['z']
        pose_msg.pose.orientation.w = pose_data['pose']['orientation']['w']

        # 发布消息
        publisher.publish(pose_msg)


def main(args=None):
    rclpy.init(args=args)

    pose_publisher = PosePublisher()

    rclpy.spin(pose_publisher)

    pose_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()
